
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_loiter.h
  * @author     baiyang
  * @date       2022-2-24
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <ahrs/ahrs_view.h>
#include <common/gp_defines.h>
#include <common/gp_math/gp_mathlib.h>
#include <mc_position_control/mc_position_control.h>
#include <mc_attitude_control/mc_attitude_control.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
/** @ 
  * @brief  
  */
typedef struct {
    ahrs_view*     _ahrs;
    Position_ctrl* _pos_control;
    Attitude_ctrl* _attitude_control;

    // parameters
    Param_float    _angle_max;             // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max
    Param_float    _speed_cms;             // maximum horizontal speed in cm/s while in loiter
    Param_float    _accel_cmss;            // loiter's max acceleration in cm/s/s
    Param_float    _brake_accel_cmss;      // loiter's acceleration during braking in cm/s/s
    Param_float    _brake_jerk_max_cmsss;
    Param_float    _brake_delay;           // delay (in seconds) before loiter braking begins after sticks are released

    // loiter controller internal variables
    Vector2f_t     _desired_accel;         // slewed pilot's desired acceleration in lat/lon frame
    Vector2f_t     _predicted_accel;
    Vector2f_t     _predicted_euler_angle;
    Vector2f_t     _predicted_euler_rate;
    uint32_t       _brake_timer;           // system time that brake was initiated
    float          _brake_accel;           // acceleration due to braking from previous iteration (used for jerk limiting)
} mc_loiter;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// Constructor
void loiter_ctor(mc_loiter* loiter, ahrs_view* ahrs, Position_ctrl* pos_control, Attitude_ctrl* attitude_control);
void loiter_init_target(mc_loiter* loiter);
void loiter_init_target2(mc_loiter* loiter, const Vector2f_t* position);
void loiter_init_target3(mc_loiter* loiter, const Vector3f_t* position);
void loiter_soften_for_landing(mc_loiter* loiter);
void loiter_set_pilot_desired_acceleration(mc_loiter* loiter, float euler_roll_angle_cd, float euler_pitch_angle_cd);
void loiter_get_stopping_point_xy(mc_loiter* loiter, Vector2f_t* stopping_point);
float loiter_get_angle_max_cd(mc_loiter* loiter);
void loiter_update(mc_loiter* loiter, bool avoidance_on);


/// get horizontal distance to loiter target in cm
static inline float loiter_get_distance_to_target(mc_loiter* loiter) { return posctrl_get_pos_error_xy_cm(loiter->_pos_control); }

/// get bearing to target in centi-degrees
static inline int32_t loiter_get_bearing_to_target(mc_loiter* loiter) { return posctrl_get_bearing_to_target_cd(loiter->_pos_control); }

/// clear pilot desired acceleration
static inline void loiter_clear_pilot_desired_acceleration(mc_loiter* loiter) { vec2_zero(&loiter->_desired_accel); }

/// get desired roll, pitch which should be fed into stabilize controllers
static inline float loiter_get_roll(mc_loiter* loiter) { return posctrl_get_roll_cd(loiter->_pos_control); }
static inline float loiter_get_pitch(mc_loiter* loiter) { return posctrl_get_pitch_cd(loiter->_pos_control); }
static inline Vector3f_t loiter_get_thrust_vector(mc_loiter* loiter) { return posctrl_get_thrust_vector(loiter->_pos_control); }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



